#include <opencv2/opencv.hpp>
#include <queue>
#include <vector>
#include <iostream>
#include <unordered_map>
#include <cmath>

struct Node {
    cv::Point pos;
    double g, h;
    Node* parent;
    Node(cv::Point p, double g_, double h_, Node* par = nullptr)
        : pos(p), g(g_), h(h_), parent(par) {}
    double f() const { return g + h; }
};

struct CompareNode {
    bool operator()(const Node* a, const Node* b) { return a->f() > b->f(); }
};

// 方向偏移 (8个方向：左右上下+对角线)
const std::vector<cv::Point> directions = {
    {1, 0}, {-1, 0}, {0, 1}, {0, -1},
    {1, 1}, {1, -1}, {-1, 1}, {-1, -1}
};

// 检查点是否在地图范围内且是可行区域
bool isValid(const cv::Mat& map, const cv::Point& pt) {
    return pt.x >= 0 && pt.y >= 0 && pt.x < map.cols && pt.y < map.rows && map.at<uchar>(pt) == 255;
}

// 计算启发式（欧几里得距离）
double heuristic(const cv::Point& a, const cv::Point& b) {
    return std::hypot(a.x - b.x, a.y - b.y);
}

// 检查是否为强迫邻居（JPS 关键步骤）
bool hasForcedNeighbor(const cv::Mat& map, const cv::Point& current, const cv::Point& direction) {
    cv::Point left(-direction.y, direction.x); // 垂直方向1
    cv::Point right(direction.y, -direction.x); // 垂直方向2
    return (!isValid(map, current + left) && isValid(map, current + left + direction)) ||
           (!isValid(map, current + right) && isValid(map, current + right + direction));
}

// 跳点扩展
cv::Point jump(const cv::Mat& map, const cv::Point& current, const cv::Point& direction, const cv::Point& goal) {
    cv::Point next = current + direction;
    if (!isValid(map, next)) return {-1, -1}; // 无效点返回
    if (next == goal || hasForcedNeighbor(map, next, direction)) return next;
    
    // 对角线方向需要递归检查直线方向是否能跳
    if (direction.x != 0 && direction.y != 0) {
        if (jump(map, next, {direction.x, 0}, goal) != cv::Point(-1, -1) ||
            jump(map, next, {0, direction.y}, goal) != cv::Point(-1, -1)) {
            return next;
        }
    }
    return jump(map, next, direction, goal);
}

// JPS 主算法
std::vector<cv::Point> JPS(const cv::Mat& map, const cv::Point& start, const cv::Point& goal) {
    std::priority_queue<Node*, std::vector<Node*>, CompareNode> openSet;
    std::unordered_map<int, Node*> visited;
    
    auto hashPoint = [&](const cv::Point& p) { return p.y * map.cols + p.x; };
    openSet.push(new Node(start, 0, heuristic(start, goal)));
    
    while (!openSet.empty()) {
        Node* current = openSet.top();
        openSet.pop();
        
        if (current->pos == goal) { // 目标点找到
            std::vector<cv::Point> path;
            while (current) {
                path.push_back(current->pos);
                current = current->parent;
            }
            std::reverse(path.begin(), path.end());
            return path;
        }
        
        if (visited.count(hashPoint(current->pos))) continue;
        visited[hashPoint(current->pos)] = current;
        
        for (const auto& dir : directions) {
            cv::Point jumpPoint = jump(map, current->pos, dir, goal);
            if (jumpPoint != cv::Point(-1, -1) && !visited.count(hashPoint(jumpPoint))) {
                openSet.push(new Node(jumpPoint, current->g + heuristic(current->pos, jumpPoint), heuristic(jumpPoint, goal), current));
            }
        }
    }
    return {}; // 无路径返回空
}

// 可视化路径
void drawPath(cv::Mat& map, const std::vector<cv::Point>& path) {
    for (const auto& p : path) map.at<uchar>(p) = 127;
}

int main() {
    cv::Mat map = cv::imread("/home/xxty/trobot/trobot_ros1/maps/规划测试1/map.png", cv::IMREAD_GRAYSCALE);
    if (map.empty()) {
        std::cerr << "Failed to load map!\n";
        return -1;
    }
    cv::threshold(map, map, 127, 255, cv::THRESH_BINARY);
    
    cv::Point start(10, 10), goal(90, 90);
    std::vector<cv::Point> path = JPS(map, start, goal);
    
    if (!path.empty()) {
        drawPath(map, path);
        cv::imshow("JPS Path", map);
        cv::waitKey(0);
    } else {
        std::cout << "No path found!" << std::endl;
    }
    return 0;
}


// g++ -o jps jps.cpp `pkg-config --cflags --libs opencv4`
